#include <iostream>
#include "Vertice.h"
#include "Triangulo.h"
#include "Nodo.h"
#include "ArbolRitter.h"
#include "colisionTriangulos.h"

using namespace std;

/***Prototipos***/
vector <Triangulo> crearVectorPrueba();


int main (int argc, char *argv[]){
    vector <Triangulo> tri = crearVectorPrueba();
    ArbolRitter arb(tri);
    Nodo R = arb.getRaiz();
    cout << "Esfera Raiz: " << endl;
    cout << "Centro: " << R.getEsfera().getCentro().getX() << "," << R.getEsfera().getCentro().getY() << "," << R.getEsfera().getCentro().getZ() << endl;
    cout << "Radio: " << R.getEsfera().getRadio() << endl;
    cout << "Hay colision: " << arb.hayColision(Triangulo(new Vertice(4.5,8,1),new Vertice(6,8,1),new Vertice(5,8,-0.5))) << endl;   
    return 0;
}

void pruebaColisionArbol(){
    vector <Triangulo> tri = crearVectorPrueba();
    ArbolRitter arb(tri);
    Nodo R = arb.getRaiz();
    cout << "Esfera Raiz: " << endl;
    cout << "Centro: " << R.getEsfera().getCentro().getX() << "," << R.getEsfera().getCentro().getY() << "," << R.getEsfera().getCentro().getZ() << endl;
    cout << "Radio: " << R.getEsfera().getRadio() << endl;
    cout << arb.hayColision(Triangulo(new Vertice(0,0,1),new Vertice(1,0,1),new Vertice(1,0.5,0.5)));
}
vector <Triangulo> crearVectorPrueba(){
    vector <Triangulo> triang(40);
    triang[0] = Triangulo (new Vertice(5,5,0),new Vertice(4,3,0),new Vertice(6,2,0));//1
    triang[1] = Triangulo (new Vertice(5,5,0),new Vertice(4,3,0),new Vertice(2,7,0));
    triang[2] = Triangulo (new Vertice(5,5,0),new Vertice(2,7,0),new Vertice(4,7,0));
    triang[3] = Triangulo (new Vertice(5,5,0),new Vertice(4,7,0),new Vertice(6,8,0));
    triang[4] = Triangulo (new Vertice(5,5,0),new Vertice(6,8,0),new Vertice(7,6,0));//5
    triang[5] = Triangulo (new Vertice(5,5,0),new Vertice(7,6,0),new Vertice(8,3,0));
    triang[6] = Triangulo (new Vertice(5,5,0),new Vertice(8,3,0),new Vertice(6,2,0));
    triang[7] = Triangulo (new Vertice(6,2,0),new Vertice(8,3,0),new Vertice(7,0,0));
    triang[8] = Triangulo (new Vertice(8,3,0),new Vertice(9,0,0),new Vertice(7,0,0));
    triang[9] = Triangulo (new Vertice(8,3,0),new Vertice(10,3,0),new Vertice(9,0,0));//10
    triang[10] = Triangulo (new Vertice(8,3,0),new Vertice(11,5,0),new Vertice(10,3,0));
    triang[11] = Triangulo (new Vertice(8,3,0),new Vertice(9,6,0),new Vertice(11,5,0));
    triang[12] = Triangulo (new Vertice(8,3,0),new Vertice(7,6,0),new Vertice(9,6,0));
    triang[13] = Triangulo (new Vertice(7,6,0),new Vertice(8,8,0),new Vertice(9,6,0));
    triang[14] = Triangulo (new Vertice(7,6,0),new Vertice(7,9,0),new Vertice(8,8,0));//15
    triang[15] = Triangulo (new Vertice(7,6,0),new Vertice(6,8,0),new Vertice(7,9,0));
    triang[16] = Triangulo (new Vertice(6,8,0),new Vertice(4,9,0),new Vertice(7,9,0));
    triang[17] = Triangulo (new Vertice(4,9,0),new Vertice(6,10,0),new Vertice(7,9,0));
    triang[18] = Triangulo (new Vertice(4,7,0),new Vertice(4,9,0),new Vertice(6,8,0));
    triang[19] = Triangulo (new Vertice(4,7,0),new Vertice(3,8,0),new Vertice(4,9,0));//20
    triang[20] = Triangulo (new Vertice(3,8,0),new Vertice(1,9,0),new Vertice(4,9,0));
    triang[21] = Triangulo (new Vertice(3,8,0),new Vertice(2,8,0),new Vertice(1,9,0));
    triang[22] = Triangulo (new Vertice(3,8,0),new Vertice(2,7,0),new Vertice(2,8,0));
    triang[23] = Triangulo (new Vertice(3,8,0),new Vertice(4,7,0),new Vertice(2,7,0));
    triang[24] = Triangulo (new Vertice(2,7,0),new Vertice(4,3,0),new Vertice(0,6,0));//25
    triang[25] = Triangulo (new Vertice(2,7,0),new Vertice(0,6,0),new Vertice(2,8,0));
    triang[26] = Triangulo (new Vertice(2,8,0),new Vertice(0,6,0),new Vertice(1,9,0));
    triang[27] = Triangulo (new Vertice(1,9,0),new Vertice(0,6,0),new Vertice(0,9,0));
    triang[28] = Triangulo (new Vertice(0,6,0),new Vertice(4,3,0),new Vertice(1,4,0));
    triang[29] = Triangulo (new Vertice(0,6,0),new Vertice(1,4,0),new Vertice(0,3,0));//30
    triang[30] = Triangulo (new Vertice(1,4,0),new Vertice(2,2,0),new Vertice(0,3,0));
    triang[31] = Triangulo (new Vertice(1,4,0),new Vertice(4,3,0),new Vertice(2,2,0));
    triang[32] = Triangulo (new Vertice(4,3,0),new Vertice(5,0,0),new Vertice(2,2,0));
    triang[33] = Triangulo (new Vertice(4,3,0),new Vertice(6,2,0),new Vertice(5,0,0));
    triang[34] = Triangulo (new Vertice(5,0,0),new Vertice(6,2,0),new Vertice(7,0,0));//35
    triang[35] = Triangulo (new Vertice(5,0,0),new Vertice(2,0,0),new Vertice(2,2,0));
    triang[36] = Triangulo (new Vertice(0,0,0),new Vertice(1,1,0),new Vertice(2,0,0));
    triang[37] = Triangulo (new Vertice(0,0,0),new Vertice(0,3,0),new Vertice(1,1,0));
    triang[38] = Triangulo (new Vertice(1,1,0),new Vertice(0,3,0),new Vertice(2,2,0));
    triang[39] = Triangulo (new Vertice(1,1,0),new Vertice(2,2,0),new Vertice(2,0,0));//40
    
    return triang;
}
void foo(){
    /* Nodo R;
       
    Punto a(1,1,1);
    Punto b(2,2,2);
    Punto c(3,3,3);
    Punto d(4,4,4);
    Punto e(5,5,5);
    Punto f(6,6,6);
    Punto g(7,7,7);
    Punto h(8,8,8);
    Punto j(9,9,9);
    Triangulo prim(a,a,a);
    Triangulo seg(b,b,b);
    Triangulo terc(c,c,c);
    Triangulo cuart(d,d,d);
    Triangulo quint(e,e,e);
    Triangulo sext(f,f,f);
    Triangulo sept(g,g,g);
    Triangulo oct(h,h,h);
    Triangulo nov(j,j,j);
    vector <Triangulo> triangs(36);
    for(int i = 0; i < 4; i++){
        triangs[i] = prim;
        triangs[i+4] = seg;
        triangs[i+8] = terc;
        triangs[i+12] = cuart;
        triangs[i+16] = quint;
        triangs[i+20] = sext;
        triangs[i+24] = sept;
        triangs[i+28] = oct;
        triangs[i+32] = nov;
        }
    vector <Triangulo> triangs(1);
    Punto t1a(0,0,0);
    Punto t1b(0,1,0);
    Punto t1c(0,0,1);
    Punto t2a(1,0.1,0);
    Punto t2b(1,0.1,0.5);
    Punto t2c(-0.01,1,0);
    Triangulo primero(t1a,t1b,t1c);
    triangs[0] = primero;
    Triangulo segundo(t2a,t2b,t2c);
    ArbolRitter arb(triangs);
    R = arb.getRaiz();
   
    cout << "haynterseccion: "  << arb.hayColision(segundo) << endl;
    Triangulo t = *arb.getMapa()[R->getNodo(3).getNodo(1).getNodo(4).getEsfera().getCentro()];
    cout << "Raiz" << endl;
    cout << "x: " <<  R->getEsfera().getCentro().getX() << " y: " <<  R->getEsfera().getCentro().getY() << " z: " <<  R->getEsfera().getCentro().getZ() << endl;
    cout << "radio: " << R->getEsfera().getRadio() << endl << endl;
    cout << "raiz - nodo 1" << endl;
    cout << "x: " <<  R->getNodo(3).getNodo(1).getEsfera().getCentro().getX() << " y: " <<  R->getNodo(3).getNodo(1).getEsfera().getCentro().getY() << " z: " <<  R->getNodo(3).getNodo(1).getEsfera().getCentro().getZ() << endl;
    cout << "radio: " << R->getNodo(3).getNodo(1).getEsfera().getRadio() << endl << endl;
    cout << "triangulo" << endl;
    cout << "x: " << t.getPunto(1).getX() << "y: " << t.getPunto(1).getY() << "z: " << t.getPunto(1).getZ() << endl;
    cout << "x: " << t.getPunto(2).getX() << "y: " << t.getPunto(2).getY() << "z: " << t.getPunto(2).getZ() << endl;
    cout << "x: " << t.getPunto(3).getX() << "y: " << t.getPunto(3).getY() << "z: " << t.getPunto(3).getZ() << endl;

    cout << R->getNodo(3).getRama(2) << endl;*/
}
